/**
****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       gimbal_task.c/h
  * @brief      2025赛季哨兵云台任务
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *  V1.1.0     Nov-11-2019     RM              1. 添加一些注释
  *  V2.0.0     Jan-1-2024      申欣宇           1. 优化了手动操作下云台的控制以及上电校准
  *  V3.0.0     Dec-20-2024     吴森桦           1. 添加自瞄功能和巡逻功能
  *                             冉文治           2. 添加大yaw辅助雷达进行导航
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#ifndef GIMBAL_TASK_H
#define GIMBAL_TASK_H

#include "main.h"
#include "CAN_receive.h"
#include "pid.h"
#include "remote_control.h"
#include "usb_task.h"
#include "shoot.h"

#define PITCH_CV_SPEED_PID_KP               8000.0f
#define PITCH_CV_SPEED_PID_KI               100.0f
#define PITCH_CV_SPEED_PID_KD               1000.0f
#define PITCH_CV_SPEED_PID_MAX_OUT          30000.0f
#define PITCH_CV_SPEED_PID_MAX_IOUT         3000.0f
#define YAW_CV_SPEED_PID_KP                 8000.0f
#define YAW_CV_SPEED_PID_KI                 100.0f
#define YAW_CV_SPEED_PID_KD                 500.0f
#define YAW_CV_SPEED_PID_MAX_OUT            30000.0f
#define YAW_CV_SPEED_PID_MAX_IOUT           2000.0f

#define PITCH_CV_ANGLE_PID_KP               25.0f
#define PITCH_CV_ANGLE_PID_KI               0.0f
#define PITCH_CV_ANGLE_PID_KD               0.0f
#define PITCH_CV_ANGLE_PID_MAX_OUT          10.0f
#define PITCH_CV_ANGLE_PID_MAX_IOUT         0.0f
#define YAW_CV_ANGLE_PID_KP                 20.0f
#define YAW_CV_ANGLE_PID_KI                 0.0f
#define YAW_CV_ANGLE_PID_KD                 0.0f
#define YAW_CV_ANGLE_PID_MAX_OUT            10.0f
#define YAW_CV_ANGLE_PID_MAX_IOUT           0.0f

// pitch 速度环 PID参数以及 PID最大输出，积分输出
#define PITCH_GYRO_SPEED_PID_KP				4000.0f
#define PITCH_GYRO_SPEED_PID_KI				0.0f
#define PITCH_GYRO_SPEED_PID_KD				500.0f
#define PITCH_GYRO_SPEED_PID_MAX_OUT		30000.0f
#define PITCH_GYRO_SPEED_PID_MAX_IOUT		0.0f
// yaw 速度环 PID参数以及 PID最大输出，积分输出
#define YAW_GYRO_SPEED_PID_KP				8000.0f
#define YAW_GYRO_SPEED_PID_KI				100.0f
#define YAW_GYRO_SPEED_PID_KD				500.0f
#define YAW_GYRO_SPEED_PID_MAX_OUT			30000.0f
#define YAW_GYRO_SPEED_PID_MAX_IOUT			2000.0f
#define YAW_PATROL_GYRO_SPEED_PID_KP		2500.0f
#define YAW_PATROL_GYRO_SPEED_PID_KI		100.0f
#define YAW_PATROL_GYRO_SPEED_PID_KD		0.0f
#define YAW_PATROL_GYRO_SPEED_PID_MAX_OUT	30000.0f
#define YAW_PATROL_GYRO_SPEED_PID_MAX_IOUT	2000.0f

// pitch 速度环 PID参数以及 PID最大输出，积分输出
#define PITCH_ENCONDE_SPEED_PID_KP		    8000.0f
#define PITCH_ENCONDE_SPEED_PID_KI		    100.0f
#define PITCH_ENCONDE_SPEED_PID_KD		    1000.0f
#define PITCH_ENCONDE_SPEED_PID_MAX_OUT		30000.0f
#define PITCH_ENCONDE_SPEED_PID_MAX_IOUT	3000.0f
// yaw 速度环 PID参数以及 PID最大输出，积分输出
#define YAW_ENCONDE_SPEED_PID_KP			4000.0f
#define YAW_ENCONDE_SPEED_PID_KI			100.0f
#define YAW_ENCONDE_SPEED_PID_KD			500.0f
#define YAW_ENCONDE_SPEED_PID_MAX_OUT		30000.0f
#define YAW_ENCONDE_SPEED_PID_MAX_IOUT		2000.0f

// pitch 角度环 角度由陀螺仪解算 PID参数以及 PID最大输出，积分输出
#define PITCH_ABSOLUTE_ANGLE_PID_KP			15.0f
#define PITCH_ABSOLUTE_ANGLE_PID_KI			0.0f
#define PITCH_ABSOLUTE_ANGLE_PID_KD			0.1f
#define PITCH_ABSOLUTE_ANGLE_PID_MAX_OUT	10.0f
#define PITCH_ABSOLUTE_ANGLE_PID_MAX_IOUT	0.0f
// yaw 角度环 角度由陀螺仪解算 PID参数以及 PID最大输出，积分输出
#define YAW_ABSOLUTE_ANGLE_PID_KP			20.0f
#define YAW_ABSOLUTE_ANGLE_PID_KI			0.0f
#define YAW_ABSOLUTE_ANGLE_PID_KD			0.0f
#define YAW_ABSOLUTE_ANGLE_PID_MAX_OUT		10.0f
#define YAW_ABSOLUTE_ANGLE_PID_MAX_IOUT		0.0f

// pitch 角度环 角度由编码器 PID参数以及 PID最大输出，积分输出
#define PITCH_RELATIVE_ANGLE_PID_KP			25.0f
#define PITCH_RELATIVE_ANGLE_PID_KI			0.0f
#define PITCH_RELATIVE_ANGLE_PID_KD			0.0f
#define PITCH_RELATIVE_ANGLE_PID_MAX_OUT	10.0f
#define PITCH_RELATIVE_ANGLE_PID_MAX_IOUT	0.0f
// yaw 角度环 角度由编码器 PID参数以及 PID最大输出，积分输出
#define YAW_RELATIVE_ANGLE_PID_KP			20.0f
#define YAW_RELATIVE_ANGLE_PID_KI			0.0f
#define YAW_RELATIVE_ANGLE_PID_KD			0.0f
#define YAW_RELATIVE_ANGLE_PID_MAX_OUT		10.0f
#define YAW_RELATIVE_ANGLE_PID_MAX_IOUT		0.0f

// 任务初始化 空闲一段时间
#define GIMBAL_TASK_INIT_TIME				201

// yaw，pitch控制通道以及状态开关通道
#define YAW_CHANNEL							2
#define PITCH_CHANNEL						3
#define GIMBAL_MODE_CHANNEL					0

// 掉头云台速度
#define TURN_SPEED							0.04f
// 遥控器输入死区，因为遥控器存在差异，摇杆在中间，其值不一定为零
#define RC_DEADBAND							10
// 根据遥控器数值进行速度转化
#define YAW_RC_SEN							-(0.003)/660
#define PITCH_RC_SEN						-(0.003)/660
// 任务控制时间间隔
#define GIMBAL_CONTROL_TIME					1
// 电机码盘值最大以及中值
#define HALF_ECD_RANGE						4096
#define ECD_RANGE							8191
// 云台初始化回中值，允许的误差，并且在误差范围内停止一段时间以及最大时间6s后解除初始化状态
#define GIMBAL_INIT_ANGLE_ERROR				0.1f
#define GIMBAL_INIT_STOP_TIME				20
#define GIMBAL_INIT_TIME					6000
#define GIMBAL_CALI_REDUNDANT_ANGLE			0.15f
// 云台初始化回中值的速度以及控制到的角度
#define GIMBAL_INIT_PITCH_SPEED				0.008f
#define GIMBAL_INIT_YAW_SPEED				0.008f
#define INIT_YAW_SET						0.0f
#define INIT_PITCH_SET						0.0f

// 判断遥控器无输入的时间以及遥控器无输入判断，设置云台yaw回中值以防陀螺仪漂移
#define GIMBAL_MOTIONLESS_RC_DEADLINE		10
#define GIMBAL_MOTIONLESS_TIME_MAX			3000

// 电机编码值转化成角度值
#ifndef MOTOR_ECD_TO_RAD
#define MOTOR_ECD_TO_RAD					0.000766990394f
#endif

typedef enum
{
    GIMBAL_MOTOR_RAW = 0,		// 电机原始值控制
    GIMBAL_MOTOR_GYRO,			// 电机陀螺仪角度控制
	GIMBAL_MOTOR_ENCONDE,		// 电机编码值角度控制
    GIMBAL_MOTOR_CV
} gimbal_motor_mode_e;

typedef struct
{
    fp32 kp;
    fp32 ki;
    fp32 kd;

    fp32 set;
    fp32 get;
    fp32 err;

    fp32 max_out;
    fp32 max_iout;

    fp32 Pout;
    fp32 Iout;
    fp32 Dout;

    fp32 out;
} gimbal_PID_t;

typedef struct
{
    gimbal_motor_mode_e gimbal_motor_mode;
    gimbal_motor_mode_e last_gimbal_motor_mode; 

    const motor_measure_t *gimbal_motor_measure;

    gimbal_PID_t gimbal_motor_absolute_angle_pid;   
    pid_type_def gimbal_motor_absolute_gyro_speed_pid; 

    gimbal_PID_t gimbal_motor_CV_angle_pid;
    pid_type_def gimbal_motor_CV_gyro_speed_pid;

    gimbal_PID_t gimbal_motor_relative_angle_pid;
    pid_type_def gimbal_motor_relative_enconde_speed_pid;

    uint16_t offset_ecd;		    //ecd
	fp32 max_relative_angle;	    //rad
    fp32 min_relative_angle;	    //rad

    fp32 max_absolute_angle;        //rad
    fp32 min_absolute_angle;        //rad

    fp32 relative_angle;            //rad
    fp32 relative_angle_set;        //rad

    fp32 absolute_angle;            //rad
    fp32 absolute_angle_set;        //rad

	fp32 motor_gyro;                //rad/s
    fp32 motor_gyro_set;            //rad/s

	fp32 motor_enconde_speed;		//ecd/s
    fp32 motor_enconde_speed_set;	//ecd/s

	fp32 raw_cmd_current;
    fp32 current_set;
    int16_t given_current;

} gimbal_motor_t;

typedef struct
{
    const RC_ctrl_t *gimbal_rc_ctrl;

    const fp32 *gimbal_INT_angle_point;
    const fp32 *gimbal_INT_gyro_point;
    cv_ctrl_t *cv_ctrl;
    const shoot_control_t *shoot_control_measure;

    gimbal_motor_t gimbal_yaw_motor;
    gimbal_motor_t gimbal_pitch_motor;

    uint32_t gimbal_dwt_count;  // 云台任务时间间隔
    fp32 dt;

    fp32 gimbal_yaw_stop_time;
    fp32 gimnal_yaw_run_time;

    fp32 gimbal_yaw_patrol_speed;
} gimbal_control_t;

extern gimbal_control_t gimbal_control;

/**
  * @brief          返回yaw 电机数据指针
  * @author         吴森桦
  * @param          none
  * @retval         yaw电机指针
  */
extern const gimbal_motor_t *get_yaw_motor_point(void);

/**
  * @brief          返回pitch 电机数据指针
  * @author         吴森桦
  * @param          none
  * @retval         pitch电机指针
  */
extern const gimbal_motor_t *get_pitch_motor_point(void);

/**
  * @brief          云台任务，间隔 GIMBAL_CONTROL_TIME 1ms
  * @author         吴森桦
  * @param          pvParameters 空
  * @retval         none
  */
extern void gimbal_task(void const *pvParameters);

#endif
